import launch
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import IncludeLaunchDescription
from launch.substitutions import Command

import os

def generate_launch_description():
    ld = LaunchDescription()
    pose_solver_package_dir = get_package_share_directory('pose_solver')
    control_package_dir = get_package_share_directory('control')
    omni_dir = get_package_share_directory('usb_cam')

    # params_file = os.path.join(get_package_share_directory('ros_usbcapture'),'config','RosUsbCapture.yaml')
    detector_params = os.path.join(
      get_package_share_directory('deepdetector'),'config','detector_params_ov.yaml')
    container = ComposableNodeContainer(
            name='my_container',
            namespace='',
            package='rclcpp_components',
            executable='component_container',

            composable_node_descriptions=[
                # ComposableNode(
                #     package='deepdetector',
                #     plugin='wmj::DeepDetector_Node',
                #     name='deepdetector_right',
                #     remappings=
                #     [
                #         ('Armors' , "/Armors"),
                #         ('image_raw', 'right_image_raw')
                #     ],
                #     parameters=[detector_params],
                #     extra_arguments=[{'use_intra_process_comms': True}],
                #     ),
                ComposableNode(
                    package='deepdetector',
                    plugin='wmj::DeepDetector_Node',
                    name='deepdetector_left',
                    remappings=
                    [
                        ('Armors' , "/Armors"),
                        ('image_raw', 'left_image_raw')
                    ],
                    parameters=[detector_params],
                    extra_arguments=[{'use_intra_process_comms': True}],
                    ),
                ComposableNode(
                    package='ros_usbcapture',
                    plugin='wmj::camera_node',
                    name='pub_image',
                    extra_arguments=[{'use_intra_process_comms': True}]
                    )
            ],
            # output='screen',
    )

    # omni_launch = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource([omni_dir, '/launch', '/omni_camera.launch.py'])
    # )

    mono_node = Node(
        package = 'monocular',
        executable = 'monocular',
        output='screen'
    )

    topaimer_node = Node(
        package="top_aimer",
        executable="top_aimer_node",
        # output="screen"
    )

    aimer_filter_node = Node(
        package = 'aimer_filter',
        executable = 'aimer_filter',
        # output='screen'   
    )
    pose_params_file = os.path.join(get_package_share_directory('pose_solver'),'config','pose_params.yaml')
    pose_solver_left = Node(
        package="pose_solver", 
        executable="pose_solver",
        name='pose_solver_left',
        parameters=[pose_params_file],
        remappings=
        [
          ('/Top_status' , "/left/Top_status"),
          ('/setGimbalAngle' , "/left/setGimbalAngle"),
          ('/shootSome' , "/left/shootSome"),
          ('/getBulletSpeed', "/left/getBulletSpeed")
        ],
        # output='screen'
    )

    pose_solver_right = Node(
        package="pose_solver",
        executable="pose_solver",
        name='pose_solver_right',
        parameters=[pose_params_file],
        remappings=
        [
          ('/Top_status' , "/right/Top_status"),
          ('/setGimbalAngle' , "/right/setGimbalAngle"),
          ('/shootSome' , "/right/shootSome"),
          ('getBulletSpeed', "/right/getBulletSpeed")
        ],
        # output='screen'
    )

#    pose_solver_launch = IncludeLaunchDescription(
#        PythonLaunchDescriptionSource([pose_solver_package_dir, '/launch', '/pose_solver.launch.py'])
#    )
    control_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([control_package_dir, '/launch', '/control.launch.py'])
    )
    robot_description = Command(['xacro ', os.path.join(
        get_package_share_directory('infantry_description'), 'urdf', 'Swan_Twins_v2.urdf.xacro')])
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[
            {'robot_description': robot_description,
            'publish_frequency': 500.0}
        ]
    )   

    scan_node = Node(
        package = 'scan',
        executable = 'scan',
        # output='screen'
    )

    # ld.add_action(control_launch)
    ld.add_action(container)
    # ld.add_action(omni_launch)
    ld.add_action(robot_state_publisher)
    ld.add_action(mono_node)
    ld.add_action(topaimer_node)
    ld.add_action(aimer_filter_node) 
    # ld.add_action(pose_solver_right)
    ld.add_action(pose_solver_left)
    # ld.add_action(pose_solver_launch)
    # ld.add_action(scan_node)
    return ld
